/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *                      Avionics Project
 *
 * FILENAME:
 * clientTest.cxx
 *
 * PURPOSE:
 *  Provide communication test of client
 *
 * LAST MODIFIED:
 * $Date: 2005/05/03 04:39:32 $
 * $Revision: 1.1.1.1 $
 * $Author: dixonc $
 ***********************************************************************/

#include "tornadoChaser.h"

#include <signal.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h>
#include <netdb.h>
#include <errno.h>

#include <iostream>

#include "String.h"
#include "socket.h"
#include "matrix.h"
#include "timeLib.h"
#include "quaternion.h"
#include "type_defs.h"

#include "airplaneStruct.h"
#include "Thread.h"

// global variable area
ShmStruct ShmStructVar;

//
TimeStamp tsNow;
TimeStamp tsLast;

//
// Airplane simulation
//
Quaternion q(0,0,0,1);	// attitude

void exitsignal( int signal )
{
	printf("\nCaught signal %d ... exiting\n",signal);
	fflush(stdout);
}

//----------------------------------------------------------
// CommSim Class
//----------------------------------------------------------
class CommSim : public pThread
{
private:
	String port;                                            // port number
	String address;                         // address string
	Socket *socket;
	
public:
	void processPilot(const Command &cmd);

	CommSim(String name) : pThread(name) {       
		socket	= NULL;
		port		= "7070";
		address	= "";	// local host
	}

	bool connect( String addrStr, String portStr) {
		address = addrStr;
		port = portStr;

		socket = new Socket(addrStr.c_str(),portStr.c_str(), Socket::UDP); 

		// init socket
		if ( !socket->open( Socket::CLIENT ) )
			return false;

		// try to connect to host
		return connectHost();

	}

	~CommSim() { 
		if( socket != NULL)
			delete socket;
	}

	//----------------------------------------------------------
	// set memory area, only what you need
	//----------------------------------------------------------
	void setShm() {
		//cout << name << ":setShm() " << endl;
		pThread::mutex_lock();

		// translate local into shareMemory

		pThread::mutex_unlock();

	}


	//----------------------------------------------------------
	// Run function
	//----------------------------------------------------------
	void CommSim::run() {
		int client;
		int num;
		Command cmd;
		sigset_t waitSigs;
		uchar8 *rxBuf = new uchar8[ CMD_MAX_SIZE ];
		CommandQueue sockQueue;                         // parse incoming socket stream


		cout << "\t" << name << ": run()" << endl;
		tsLast.stamp();
		tsNow.stamp();

		// setup signals
		sigemptyset(&waitSigs);
		sigaddset(&waitSigs,SIGALRM);
		sigaddset(&waitSigs,SIGINT);
		sigaddset(&waitSigs,TSIGQUIT);
		sigprocmask(SIG_BLOCK,&waitSigs,NULL);

		struct sigaction sa;
		sigemptyset(&sa.sa_mask);
		sa.sa_flags = SA_SIGINFO;
		sa.sa_sigaction = sigHandler;

		if( sigaction(TSIGQUIT, &sa, NULL) < 0)
			perror("pThread::run sigaction");

		// output
		shmVar.pilot.printHead();

		while(execRate > 0) {

			if( socket->isConnected() ) {

				// wait for input, timeout at 1000 milli-sec
				client = socket->wait(1000);

				// if this is a UDP connection, then we
				// need to try to connect to the server
				if( client == Socket::WAIT_TIMEOUT) {
					cout << "socket->wait timedout" << endl;
					if( socket->getType() == Socket::UDP ) {
						socket->close();
					}

				} else if(client == Socket::WAIT_INT)
					cout << "socket->wait interrupted" << endl;

				else if(client == Socket::WAIT_ERROR)
					cout << "socket->wait error" << endl;

				// since I am a client, client should = 0
				else if( client == Socket::WAIT_PEER ) {
					// get the command from host
					num = socket->read((char *)rxBuf, CMD_MAX_SIZE);

					if( num > 0) {
						sockQueue.push(rxBuf,num);
						while( sockQueue.length() >= CMD_PACKET_SIZE) {
							if( sockQueue.isValid() ){
								Command rxCmd = sockQueue.popCommand();
								parseCommand(rxCmd);
							} else {
								cout << "-- Dropping byte --" << endl
									<< (int)sockQueue.popByte() << endl;
							}
						}
					} else {
						if( num == 0) 
							cout << "\n<--- Server has disconnected abnormally --->\n";
						else if( errno != EWOULDBLOCK || errno != EINTR) 
							perror("socket->read() - bad read from host");

						socket->close();
					}       
				}

			} else {
				// wait
				sleep(1);

				// try to connect to host
				if( true == connectHost() )
					cout << "Client connected to " << socket->get_hostname() << "\n";
			}	

		} // end of while()

		closeConnections();	
		pThread::exit();
	}

	//----------------------------------------------------------
	// connect client 
	//----------------------------------------------------------
	bool CommSim::connectHost() {
		if( !socket->isConnected() )
			socket->connectHost();

		if( socket->isConnected() ) {
			// send to host
			uchar8 cmdBuf[] = {CMD_CONNECTING, 0, 0, 0};
			Command send(cmdBuf);
			send.setCheckSum();

			if( socket->write(send) != send.size() ){
				cout << "<!!! Could not accept client connetion !!!>" <<endl; 
				return false;
			}
		} else
			return false;

		return true;
	}

	//----------------------------------------------------------
	// close client connections
	//----------------------------------------------------------
	void CommSim::closeConnections() {
		uchar8 cmdBuf[] = { CMD_DISCONNECTING, 0, 0, 0};
		Command cmd(cmdBuf);
		cmd.setCheckSum();

		cout << "closing connection ... " << endl;

		// send disconnect command to host
		if( socket->isConnected() )
			socket->write( cmd );

		// close socket
		socket->close();
		delete socket;
	}

	//----------------------------------------------------------
	// Act on received commands
	//----------------------------------------------------------
	void CommSim::parseCommand(const Command &cmd) {
		if( !cmd.isValid() ) {
			cout << "<!!! " << " CommSim::parseCommand - invalid checksum !!!>\n";
			cmd.printCmd();

			return;
		}	

		switch( cmd.cmd() ) {
		// check if connecting
		case CMD_CONNECTING:
			cout << "<--- Request from host to connect --->" << endl;
			break;

		// check if exiting
		case CMD_DISCONNECTING:
			cout << "<--- host has disconnected --->" << endl;
			// can only close the socket and try to open a new one
			socket->close();
			break;

		case CMD_PILOT:
			processPilot(cmd);		
			break;

		default:
			cout << "!!!! Unknown command " << cmd.cmd() << " !!!!" << endl;
			break;
		}
	}
};

int main(int argc, char *argv[])
{
	String hostStr = "localhost";
	String portStr = "7070";

	// used for getopt to get otions from arguments
	extern int optind;
	//extern char * optarg;

	int c;
	while ((c=getopt(argc,argv,"h")) != EOF) switch( (char)c)
	{
		case 'h':
		    	cout << "no help yet - open the file and look" << endl; 
			exit(0);
		default:
			printf("unknown arg %s\n", argv[optind - 1]);
		    	cout << "no help yet" << endl; 
			exit(EXIT_SUCCESS);
	}

	// only hostname
	if( optind + 1  ==  argc) {
		hostStr = String(argv[optind]);	
	} else if( optind + 2  ==  argc) {
		hostStr = String(argv[optind]);	
		portStr = String(argv[optind + 1]);	
	} else if( optind < argc){
		cout << "no help yet" << endl;
		exit(0);
	}

	 //---------------------------------------------------------- 
	 // Catch Ctrl-c
	 //---------------------------------------------------------- 
	 signal( SIGINT, exitsignal);

	 //---------------------------------------------------------- 
	 // threads
	 //---------------------------------------------------------- 
	 CommSim sim("sim");

	 //---------------------------------------------------------- 
	 // init the shared memory area
	 //---------------------------------------------------------- 
	 pthread_mutex_init(&ShmStructVar.mtx, NULL);

	//---------------------------------------------------------- 
	// init the different threads
	//---------------------------------------------------------- 
	String error = "";

	cout << "Initializing threads ... " << endl;
	if ( sim.init(&ShmStructVar) != true ) 
		error = "Could not start the Sim task";
 	else if ( sim.connect(hostStr,portStr) != true ) {
		cout << "Could not connect to " + hostStr + ":"+ portStr << endl;
	}
	
	if( error != "" ) {
		cout << error << endl <<  "Exiting ..." << endl;
		// let default destructors deal with closing
		// system down properly since we havent started
		// threads
		exit(EXIT_FAILURE);
	}

	//---------------------------------------------------------- 
	// Start the different threads and runrates
	//---------------------------------------------------------- 
	cout << "Starting threads ... " << endl;
	if ( sim.start(25.0) != OK )
		cout << "Sim Start Error" <<endl;

	//---------------------------------------------------------- 
	// The main task can still hang out and do stuff
	//---------------------------------------------------------- 
	pause();

	//---------------------------------------------------------- 
	// Shutdown the threads
	//---------------------------------------------------------- 
	cout << "Stopping threads" << endl;
	sim.shutdown( );

	// give them time
	sleep(1);

	//---------------------------------------------------------- 
	// Total program exit
	//---------------------------------------------------------- 
	cout << endl;
	exit(EXIT_SUCCESS); return EXIT_SUCCESS;	 
}


void CommSim::processPilot(const Command &cmd)
{

	// first get copy of command values into shared memory
	unsigned char *buf = new unsigned char[ cmd.size() ];
	cmd.copyToBuf(buf);
	shmVar.pilot.copyFromBuf( (char *) (buf+CMD_HEADER_SIZE) );

	//
	// show user pilot
	//
	shmVar.pilot.print();
	printf ( "\r" ); fflush ( stdout ) ;


	/////////////////////////////////////////
	// simulate sensor values of servos
	/////////////////////////////////////////
	shmVar.sensor.servos[0] = (shmVar.pilot.roll);
	shmVar.sensor.servos[1] = (shmVar.pilot.pitch);
	shmVar.sensor.servos[2] = (shmVar.pilot.yaw);
	shmVar.sensor.servos[3] = (shmVar.pilot.throttle);

	/////////////////////////////////////////
	// timestamp
	/////////////////////////////////////////
	float t;
	tsNow.stamp();
	t = tsNow - tsLast; // this is micro-sec
	tsLast = tsNow;

	////////////////////////////////////
	// send down plane params
	////////////////////////////////////
	delete [] buf;
	buf = new uchar[sizeof(SensorStruct) + 4];

	buf[0] = CMD_SENSOR_CMD;
	buf[1] = sizeof(SensorStruct);
	shmVar.sensor.copyToBuf( (char *)(buf+CMD_HEADER_SIZE) );
	Command send(buf);
	send.setCheckSum();
	
	if( socket->write(send) != send.size() ){
		cout << "<!!! Could not write to port !!!>" <<endl; 
	}
	//cout << send ;
	delete [] buf;
}
